/*
 *  Mikrokopter Driver
 *  Copyright (C) 2012, CYPHY lab
 *  Inkyu Sa <i.sa@qut.edu.au>
 *
 *  http://wiki.qut.edu.au/display/cyphy
 *
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "IMU.h"

using namespace std;


// Copied from FC.c
#if 0
struct str_ExternControl
{
 unsigned char Digital[2];
 unsigned char RemoteTasten;
 signed char   Nick;
 signed char   Roll;
 signed char   Gier;
 unsigned char Gas;
 signed char   Hight;
 unsigned char free;
 unsigned char Frame;
 unsigned char Config;
};
#endif


typedef struct
{
	uint8_t	Digital[2];
	uint8_t	RemoteButtons;
	int8_t	Pitch;
	int8_t	Roll;
	int8_t	Yaw;
	uint8_t	Height;
	uint8_t	free;
	uint8_t	Frame;
	uint8_t	Config;
	uint8_t dummy;
} ExternControl_t;
ExternControl_t   ExternControl;

void SendData (char *output, int len)
  {
    int i;
    i = write (fd, output, len);
    if (i != len)
    {
      
      ROS_INFO ("Error wrote %d out of %d element(s): %s", i, len, strerror (errno));
      ROS_BREAK ();
    }
  }


void AddCRC(uint16_t datalen)
{
	uint16_t tmpCRC = 0, i;
	for(i = 0; i < datalen; i++)
	{
		tmpCRC += g_txd_buffer[i];
	}
	tmpCRC %= 4096;
	g_txd_buffer[datalen++] = '=' + tmpCRC / 64;
	g_txd_buffer[datalen++] = '=' + tmpCRC % 64;
	g_txd_buffer[datalen++] = '\r';
	SendData(g_txd_buffer,datalen);
}

void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ...
{ 
	va_list ap;
	uint16_t pt = 0;
	uint8_t a,b,c;
	uint8_t ptr = 0;

	uint8_t *pdata = 0;
	int len = 0;

	g_txd_buffer[pt++] = '#';			// Start character
	g_txd_buffer[pt++] = 'a' + addr;		// Address (a=0; b=1,...)
	g_txd_buffer[pt++] = cmd;			// Command

	va_start(ap, numofbuffers);
	if(numofbuffers)
	{
		pdata = va_arg(ap, uint8_t*);
		len = va_arg(ap, int);
		ptr = 0;
		numofbuffers--;
	}

	while(len)
	{
		if(len)
		{
			a = pdata[ptr++];
			len--;
			if((!len) && numofbuffers)
			{
				pdata = va_arg(ap, uint8_t*);
				len = va_arg(ap, int);
				ptr = 0;
				numofbuffers--;
			}
		}
		else a = 0;
		if(len)
		{
			b = pdata[ptr++];
			len--;
			if((!len) && numofbuffers)
			{
				pdata = va_arg(ap, uint8_t*);
				len = va_arg(ap, int);
				ptr = 0;
				numofbuffers--;
			}
		}
		else b = 0;
		if(len)
		{
			c = pdata[ptr++];
			len--;
			if((!len) && numofbuffers)
			{
				pdata = va_arg(ap, uint8_t*);
				len = va_arg(ap, int);
				ptr = 0;
				numofbuffers--;
			}
		}
		else c = 0;
		g_txd_buffer[pt++] = '=' + (a >> 2);
		g_txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
		g_txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
		g_txd_buffer[pt++] = '=' + ( c & 0x3f);
	} //while
	va_end(ap);
	AddCRC(pt); // add checksum after data block and initates the transmission
}


void logging()
{
	char dir[100]="";
	time_t rawtime;
	struct tm * timeinfo;

	time ( &rawtime );
	timeinfo = localtime ( &rawtime );
	if (timeinfo->tm_sec<55)
	{
	sprintf ( dir ,"/home/pilot/log/%4d_%02d_%02d_%02d%02d",  timeinfo->tm_year+1900, timeinfo->tm_mon+1, timeinfo->tm_mday, timeinfo->tm_hour, timeinfo->tm_min);
	}
	else
	{
		sprintf ( dir ,"/home/pilot/log/%4d_%02d_%02d_%02d%02d",  timeinfo->tm_year+1900, timeinfo->tm_mon+1, timeinfo->tm_mday, timeinfo->tm_hour, timeinfo->tm_min+1);

	}
	mkdir(dir,0777);//creating a directory

	char log[100]="";
	char format[30]="/imu_debug_data.txt";
	strcat(log,dir);
	strcat(log,format);
	
	if((fid=fopen(log,"w+"))==NULL)
		 cout<<"Fail to open the file!"<<endl;

}

int decode(string str1){
	
	char rxd_buffer[100];
	unsigned char buffer[100];
	str1.copy(rxd_buffer,str1.length());
	//rxd_buffer[83]=0;
	//buffer[83]=0;
//	cout<<"\nReceived string: "<<rxd_buffer<<endl;
	unsigned char a,b,c,d;
	int ptrIn = 3;
	int ptrOut = 3;
	int len = str1.length()+1-6;
	buffer[0]='#';
	buffer[1]='#';
	buffer[2]='#';
	while (len)
	{
		a = rxd_buffer[ptrIn++] - '=';
		b = rxd_buffer[ptrIn++] - '=';
		c = rxd_buffer[ptrIn++] - '=';
		d = rxd_buffer[ptrIn++] - '=';

		unsigned char x = (a << 2) | (b >> 4);
		unsigned char y = ((b & 0x0f) << 4) | (c >> 2);
		unsigned char z = ((c & 0x03) << 6) | d;
		if (len--){
			buffer[ptrOut++] = x;
		}
		else
			break;
		if (len--){
			buffer[ptrOut++] = y; 
		}
		else
			break;
		if (len--){
			buffer[ptrOut++] = z;
		}
		else
			break;
	}

	int mm=0;
	int tmp_buf=0;
	len = str1.length()+1-6;
	for (int i=0;mm<=15;){
		 
		if (buffer[5+i+1]>128)
			pbuf[mm++] = buffer[5+i]-256-(255-buffer[5+i+1])*256;	
		else
			pbuf[mm++] = buffer[5+i]+buffer[5+i+1]*256;
		i=i+2;
		if ((seqs==0)&&(mm-1<9))
		{
			bias_pbuf[mm-1]=pbuf[mm-1];
			seqs++;
		//	bias_pbuf[mm-1]=bias_pbuf[mm-1]/2;	
			
		}


		if ((mm==3)||(mm==4))
		{
			pbuf[mm-1]=pbuf[mm-1]-pbuf[mm-3]-bias_pbuf[mm-1];

		}
		else
		{
			pbuf[mm-1]=pbuf[mm-1]-bias_pbuf[mm-1];
		}
				

			

		switch(mm-1)
		{
			case 0:	msg.data.push_back(pbuf[mm-1]); msg_imu.data.push_back(pbuf[mm-1]); break;
			case 1: msg.data.push_back(pbuf[mm-1]);	msg_imu.data.push_back(pbuf[mm-1]); break;
			case 11: msg.data.push_back(pbuf[mm-1]); msg_imu.data.push_back(pbuf[mm-1]); break;
			default: msg.data.push_back(pbuf[mm-1]);
		}
	}


	return 1;
	
}

void mikoHeightCmdCallback(const imu::mikoCmd& msg)
{
//		ROS_INFO("heightCmdCallback");
		ExternControl.Height = (uint8_t)msg.throttle;
		if(ExternControl.Height<= 0) ExternControl.Height =0;
		if(ExternControl.Height >=180) ExternControl.Height = 180;
		SendOutData('b', FC_ADDRESS,1,(uint8_t*)&ExternControl, sizeof(ExternControl));
		printf("height callback r,p,y,t= %d %d %d %d\n",ExternControl.Roll,ExternControl.Pitch,ExternControl.Yaw,ExternControl.Height);		
//		cout<<"throttle = "<<ExternControl.Height<<endl;
}

void mikoCmdCallback(const imu::mikoCmd& msg)
{
//         	 ROS_INFO("CmdCallback");
		 //ROS_INFO("msg.throttle = %d",msg.throttle);
		 ExternControl.Pitch = msg.pitch;
		 ExternControl.Roll = msg.roll;
		 ExternControl.Yaw = msg.yaw;	 
		 ExternControl.Height = (uint8_t)msg.throttle;
		
		if(ExternControl.Height<= 0) ExternControl.Height =0;
		if(ExternControl.Height >=180) ExternControl.Height = 180;



		 int cmd_max=100;
		 if(ExternControl.Pitch >=cmd_max) ExternControl.Pitch=cmd_max;
		 if(ExternControl.Pitch <=-cmd_max) ExternControl.Pitch=-cmd_max;

		 if(ExternControl.Roll >=cmd_max) ExternControl.Roll=cmd_max;
		 if(ExternControl.Roll <=-cmd_max) ExternControl.Roll=-cmd_max;
		 
		 if(ExternControl.Yaw >=60) ExternControl.Yaw=60;
		 if(ExternControl.Yaw <=-60) ExternControl.Yaw=-60;

		 

		 printf("cmd call back r,p,y,t= %d %d %d %d\n",ExternControl.Roll,ExternControl.Pitch,ExternControl.Yaw,ExternControl.Height);
//		 cout<<"throttle = "<<(unsigned char)ExternControl.Height<<endl;

		 SendOutData('b', FC_ADDRESS,1,(uint8_t*)&ExternControl, sizeof(ExternControl));
	}

/**
 * This tutorial demonstrates simple sending of messages over the ROS system.
 */
int main(int argc, char **argv)
{
//	logging();
	int start;
//	//fseek(fid,0,SEEK_END);
//	//fprintf(fid,"%s;\n","TimeStamp, AnglePitch, AngleRoll, GyroPitch, GyroRoll, GyroYaw, Height, AccZ, Thrust, Compass, Volt, Receiver, Gyrocompass, Motor1-4");

	fd=open(IMU_PORT, O_RDWR | O_NOCTTY);
	if (fd<0){
		perror(IMU_PORT);
		return -1;
	}
	tcgetattr(fd,&oldtio); /* save current serial port settings */
        //bzero(&newtio, sizeof(newtio)); /* clear struct for new port settings */

	/* 
          BAUDRATE: Set bps rate. You could also use cfsetispeed and cfsetospeed.
          CRTSCTS : output hardware flow control (only used if the cable has
                    all necessary lines. See sect. 7 of Serial-HOWTO)
          CS8     : 8n1 (8bit,no parity,1 stopbit)
          CLOCAL  : local connection, no modem contol
          CREAD   : enable receiving characters
        */
         newtio.c_cflag = BAUDRATE | CRTSCTS | CS8 | CLOCAL | CREAD;

	/*
          IGNPAR  : ignore bytes with parity errors
          ICRNL   : map CR to NL (otherwise a CR input on the other computer
                    will not terminate input)
          otherwise make device raw (no other input processing)
        */
         newtio.c_iflag = IGNPAR | ICRNL;

	/*
         Raw output.
        */
         newtio.c_oflag = 0;
         
        /*
          ICANON  : enable canonical input
          disable all echo functionality, and don't send signals to calling program
        */
         newtio.c_lflag = ICANON;
	
	/* 
          initialize all control characters 
          default values can be found in /usr/include/termios.h, and are given
          in the comments, but we don't need them here
        */
         newtio.c_cc[VINTR]    = 0;     /* Ctrl-c */ 
         newtio.c_cc[VQUIT]    = 0;     /* Ctrl-\ */
         newtio.c_cc[VERASE]   = 0;     /* del */
         newtio.c_cc[VKILL]    = 0;     /* @ */
         newtio.c_cc[VEOF]     = 4;     /* Ctrl-d */
         newtio.c_cc[VTIME]    = 0;     /* inter-character timer unused */
         newtio.c_cc[VMIN]     = 1;     /* blocking read until 1 character arrives */
         newtio.c_cc[VSWTC]    = 0;     /* '\0' */
         newtio.c_cc[VSTART]   = 0;     /* Ctrl-q */ 
         newtio.c_cc[VSTOP]    = 0;     /* Ctrl-s */
         newtio.c_cc[VSUSP]    = 0;     /* Ctrl-z */
         newtio.c_cc[VEOL]     = 0;     /* '\0' */
         newtio.c_cc[VREPRINT] = 0;     /* Ctrl-r */
         newtio.c_cc[VDISCARD] = 0;     /* Ctrl-u */
         newtio.c_cc[VWERASE]  = 0;     /* Ctrl-w */
         newtio.c_cc[VLNEXT]   = 0;     /* Ctrl-v */
         newtio.c_cc[VEOL2]    = 0;     /* '\0' */
        
        /* 
          now clean the modem line and activate the settings for the port
        */
         tcflush(fd, TCIFLUSH);
         tcsetattr(fd,TCSANOW,&newtio);
	 fcntl(fd, F_SETFL, FNDELAY);



	ros::init(argc, argv, "imu_debug");
	ros::NodeHandle imu;




	ros::Publisher imu_pub = imu.advertise<std_msgs::Float32MultiArray>("imu_debug", 1);
	ros::Publisher mk_imu_pub = imu.advertise<sensor_msgs::Imu>("mk_imu", 1);
	ros::Subscriber mikoCmdSubscriber = imu.subscribe ("mikoCmd", 1, &mikoCmdCallback);
	ros::Subscriber mikoHeightCmdSubscriber = imu.subscribe ("mikoHeightCmd", 1, &mikoHeightCmdCallback);
	ros::Publisher stick_cmd_pub = imu.advertise<geometry_msgs::TwistStamped>("stick_cmd", 1);

	ros::Rate loop_rate(1000);
//	ros::Rate loop_rate(70);
	int count = 0;

	double t_dif1=0, t_dif2=0;
	int res=-1;
	char buf[170];
	int cnt=0;
	int frq=0;
        int fail=0;
	double t_dif=0;
	time(&t_start); 
	time(&t1_start);  
		

	int out=0;
	write(fd,"#ad>M==Dj\r",sizeof("#ad>M==Dj\r")/sizeof(char));
	while (ros::ok())
	//while (1)
	{
		//ROS_INFO("ros::ok");
			while (1)
			{
				res=read(fd,buf,170);
				if (res!=-1)
				{
					buf[res]=0;
					string gs(buf);
					frq++;
					//cout<<gs<<endl;
					if ((gs.find("#bD")==0))
					{//&&(gs.find('\r')==0)){
						

						unsigned int sum=0,i;
						for (i=0;i<gs.length()-3;i++)
						{
							sum += buf[i];
						}

						sum %=4096;
				
						if (((buf[i]-'='-sum/64)==0)&&((buf[i+1]-'='-sum%64)==0))
						{
							msg.data.clear();
							msg_imu.data.clear();
							out=decode(gs);
							ROS_IMU_Msgs();
							imu_pub.publish(msg);
							mk_imu_pub.publish(ros_imu_msg);
							ROS_Stick_Msgs();
							stick_cmd_pub.publish(msg_stick_cmd);
							//pose_pub.publish(msg_imu);
							
							cnt++;
							
						}
						else
						{
							//cout<<"IMU data chech sum not passed"<<endl;
							fail++;
						}
					
					}

				}
				if (frq>200)
				{
					cout<<"Send! "<<frq<<endl;
					write(fd,"#ad>M==Dj\r",sizeof("#ad>M==Dj\r")/sizeof(char));
					frq=0;
				}
				time(&t_end);
				t_dif = difftime(t_end,t_start);
				if (t_dif>=1)
				{
					cout<<"Updating rate: "<<cnt<<" Packets lost: "<<fail<<endl;
					fail=0;
				//	frq=cnt;
					if (cnt<60)
					{
						cout<<"cnt<60, Send!"<<endl;
						write(fd,"#ad>M==Dj\r",sizeof("#ad>M==Dj\r")/sizeof(char));
					}
					cnt=0;
					time(&t_start);
				}
				if (out==1)
				{
					//ROS_INFO("out");
					out=0;	
					break;
				}
				ros::Duration(0.001).sleep();
				if(ros::isShuttingDown()) 
				{
					ROS_INFO("Shut down");
					break;
				}
			}//while(1)


    ros::spinOnce();
    loop_rate.sleep();

    ++count;
  }//	while (ros::ok())

  return 0;
}

//==========================================
// IMU data structure from Flight control
//==========================================

//  data[0] | data[1] | data[2] | ...
//  pitch,  | roll    | ACCX    | ACCY  | Yaw rate | Barometer| 
//  Deg*10


void get_data(double* accel, double* gyro,double* angle)
{
	// Get angles
	*angle=deg2rad(-msg.data[1]/10); //Roll angle
	angle++;
	*angle=deg2rad(-msg.data[0]/10); //Pitch angle
	angle++;
	double temp;
	temp=deg2rad(msg.data[11]); //Yaw angle 
	// To make IMU Yaw coordinate same as Laser Yaw coordinate.
	if(temp>=PI) *angle=(double)(temp-2*PI);
	else *angle= (double)temp;
	
	// Get Gyros
	*gyro=deg2rad(-msg.data[10]*gyro_scale/10); //Roll rate
	gyro++;
	*gyro=deg2rad(-msg.data[9]*gyro_scale/10); //Pitch rate
	gyro++;
	*gyro=deg2rad(msg.data[4]*gyro_scale/10); //Yaw rate

	// Get Accelerations
	*accel=-(msg.data[2]/606.)*g; //x axis
	accel++;
	*accel=(msg.data[3]/603.4)*g; //y axis
	accel++;
	*accel=(-msg.data[6]/15.5+g); //z axis	 
}

void ROS_IMU_Msgs()
{
	double accel[3];
	double gyro[3];
	double angle[3];

	get_data(accel,gyro,angle);

/*
	cout<<"angle[0] = "<<rad2deg(angle[0])<<"  angle[1] = "<<rad2deg(angle[1])
                           <<"  angle[2] = "<<rad2deg(angle[2])<<endl;

	cout<<"gyro[0] = "<<rad2deg(gyro[0])<<"  gyro[1] = "<<rad2deg(gyro[1])
                           <<"  gyro[2] = "<<rad2deg(gyro[2])<<endl;
	cout<<"accel[0] = "<<accel[0]<<"  accel[1] = "<<accel[1]
                         <<"  accel[2] = "<<accel[2]<<endl;
*/

	//Publish angles as Quaternion form.	
	/*
 	btQuaternion q;
	q.setEulerZYX(angle[2],angle[1],angle[0]);
	tf::quaternionTFToMsg(q,ros_imu_msg.orientation);
	*/

	ros_imu_msg.linear_acceleration.x = accel[0];
	ros_imu_msg.linear_acceleration.y = accel[1];
	ros_imu_msg.linear_acceleration.z = accel[2];

	ros_imu_msg.angular_velocity.x = gyro[0];
	ros_imu_msg.angular_velocity.y = gyro[1];
	ros_imu_msg.angular_velocity.z = gyro[2];

	ros_imu_msg.orientation.x = angle[0];
	ros_imu_msg.orientation.y = angle[1];
	ros_imu_msg.orientation.z = angle[2];
	ros_imu_msg.orientation.w = 0.5;
	ros_imu_msg.header.stamp = ros::Time::now();
}

void ROS_Stick_Msgs()
{
	msg_stick_cmd.header.stamp = ros::Time::now();
	msg_stick_cmd.twist.linear.x=msg.data[13];
	msg_stick_cmd.twist.linear.y=msg.data[12];
	msg_stick_cmd.twist.linear.z=msg.data[15];
	msg_stick_cmd.twist.angular.z=msg.data[14];
}

